/* Includes ------------------------------------------------------------------*/
#include "mpu6050.h"
#include "gpio.h"
#include "oled.h"

/*----------------------------------------------------------------------------*/
/* Configure MPU6050                                                             */
/*----------------------------------------------------------------------------*/

uint8_t mpu_slave_addr = 0xd0;

void mpu_delay() {
  uint8_t i = 100;

  while (i) {
    i--;
  }
}

void MPU6050_sda_out(uint8_t out) {
  GPIO_InitTypeDef GPIO_InitStruct;

  HAL_GPIO_WritePin(mpu_GPIO_Port, mpu_sda_Pin, GPIO_PIN_SET);

  GPIO_InitStruct.Pin = mpu_sda_Pin;
  if (out) {
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  } else {
    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  }

  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(mpu_GPIO_Port, &GPIO_InitStruct);
}

void MPU6050_IIC_Start(void) {
  MPU6050_SCL_Set();
  MPU6050_SDA_Set();
  MPU6050_SDA_Clr();
}

void MPU6050_IIC_Stop(void) {
  MPU6050_SCL_Set();
  MPU6050_SDA_Clr();
  MPU6050_SDA_Set();
}

void MPU6050_IIC_Wait_Ack(void) {
  MPU6050_SCL_Set();
  MPU6050_SCL_Clr();
}

void MPU6050_IIC_Ack(uint8_t ack) {
  MPU6050_SCL_Clr();

  if (ack) {
    MPU6050_SDA_Clr();
  } else {
    MPU6050_SDA_Set();
  }

  MPU6050_SCL_Set();
  MPU6050_SCL_Clr();
}

void MPU6050_Write_IIC_Byte(uint8_t IIC_Byte) {
  uint8_t i;

  MPU6050_SCL_Clr();

  for (i = 0; i < 8; i++) {
    if (IIC_Byte & 0x80) {
      MPU6050_SDA_Set();
    } else {
      MPU6050_SDA_Clr();
    }
    IIC_Byte <<= 1;
    MPU6050_SCL_Set();
    MPU6050_SCL_Clr();
  }
}

uint8_t MPU6050_Read_IIC_Byte(uint8_t ack) {
  uint8_t i, receive = 0;

  MPU6050_sda_out(0);

  for (i = 0; i < 8; i++) {
    MPU6050_SCL_Clr();
    __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
    MPU6050_SCL_Set();

    receive <<= 1;

    if (MPU6050_READ_SDA) {
      receive++;
    }
  }

  MPU6050_sda_out(1);

  if (ack) {
    MPU6050_IIC_Ack(1);
  } else {
    MPU6050_IIC_Ack(0);
  }

  return receive;
}

void MPU6050_Single_Write(uint8_t addr, uint8_t data) {
  MPU6050_IIC_Start();
  MPU6050_Write_IIC_Byte(mpu_slave_addr);
  MPU6050_IIC_Wait_Ack();
  MPU6050_Write_IIC_Byte(addr);
  MPU6050_IIC_Wait_Ack();
  MPU6050_Write_IIC_Byte(data);
  MPU6050_IIC_Wait_Ack();
  MPU6050_IIC_Stop();
}

uint8_t MPU6050_Single_Read(uint8_t addr) {
  uint8_t rec;

  MPU6050_IIC_Start();
  MPU6050_Write_IIC_Byte(mpu_slave_addr);
  MPU6050_IIC_Wait_Ack();
  MPU6050_Write_IIC_Byte(addr);
  MPU6050_IIC_Wait_Ack();

  MPU6050_IIC_Start();
  MPU6050_Write_IIC_Byte(mpu_slave_addr + 1);
  MPU6050_IIC_Wait_Ack();

  rec = MPU6050_Read_IIC_Byte(0);
  MPU6050_IIC_Stop();

  return rec;
}

void MPU6050_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct;

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOB_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(mpu_GPIO_Port, mpu_scl_Pin | mpu_sda_Pin, GPIO_PIN_SET);

  /*Configure GPIO pins : PBPin PBPin */
  GPIO_InitStruct.Pin = mpu_scl_Pin | mpu_sda_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(mpu_GPIO_Port, &GPIO_InitStruct);

  MPU6050_Single_Write(PWR_MGMT_1, 0x00);
  MPU6050_Single_Write(SMPLRT_DIV, 0x07);
  MPU6050_Single_Write(CONFIG, 0x06);
  MPU6050_Single_Write(GYRO_CONFIG, 0x18);
  MPU6050_Single_Write(ACCEL_CONFIG, 0x01);
}
